import math
import numpy as np
import time
import matplotlib.pyplot as plt
import GirdMap
from NodeBase import NodeBase


class GBNN:
    def __init__(self, gridmap):
        self.beta = 0.7
        self.R = 1.8
        self.c = 0.5
        self.gridMap = gridmap
        self.points = [NodeBase(self.gridMap.MapList[i][0], self.gridMap.MapList[i][1], self.gridMap.MapList[i][2]) for
                       i in range(len(self.gridMap.MapList))]
        self.direct = [[-1, 1], [0, 1], [1, 1],
                       [-1, 0],         [1, 0],
                       [-1, -1], [0, -1], [1, -1]]
        self.start = [0, 0]

    def init_start(self):
        for i in range(len(self.points)):
            if self.points[i].x == self.start[0] and self.points[i].y == self.start[1]:
                self.points[i].oc = 1
                return i

    def run(self, index):
        self.points[index].Ii = 0
        self.F_fun(index)
        next_index = self.find_next(index, 0)
        path = [[self.points[index].x, self.points[index].y]]
        while True:

            past_index = index
            index = next_index
            self.points[index].Ii = 0
            self.F_fun(index)
            path.append([self.points[index].x, self.points[index].y])
            # print(self.points[index].f1)
            print(path[-1])
            next_index = self.find_next(index, past_index)
            if self.all_covered():
                print('ok')
                print(path)
                break
        return path

    def find_next(self, index, p_index):
        next_index = 0
        old_value = 0
        point_around = self.find_around(index)

        for i in range(len(point_around)):
            a_index = point_around[i][0] * 10 + point_around[i][1]
            delta_phi = self.de_fei(p_index, index, a_index)
            # vir_value = self.V_fun(a_index)
            new_value = self.points[a_index].f1 + self.c * (1 - delta_phi / math.pi)
            if new_value > old_value:
                next_index = a_index
                old_value = new_value

        return next_index

    def V_fun(self, index):
        point_around = self.find_around(index)
        temp_value = 0
        for i in range(len(point_around)):
            a_index = point_around[i][0] * 10 + point_around[i][1]
            temp_value = temp_value + self.W_fun(index, a_index) * self.points[a_index].f1
        vir_value = self.G_fun(temp_value)
        # print(vir_value)
        return vir_value

    def de_fei(self, p_index, index, n_index):
        phi_k = math.atan2(self.points[n_index].y - self.points[index].y, self.points[n_index].x - self.points[index].x)
        phi_c = math.atan2(self.points[index].y - self.points[p_index].y, self.points[index].x - self.points[p_index].x)
        delta_phi = math.fabs(phi_k - phi_c)

        v_1 = np.array((self.points[n_index].x - self.points[index].x, self.points[n_index].y - self.points[index].y))
        v_2 = np.array((self.points[index].x - self.points[p_index].x, self.points[index].y - self.points[p_index].y))
        l_x = np.sqrt(v_1.dot(v_1))
        l_y = np.sqrt(v_2.dot(v_2))
        if l_y == 0:
            v_2 = np.array((1, 0))
            l_y = np.sqrt(v_2.dot(v_2))
        dian = v_1.dot(v_2)
        cos_ = dian / (l_x * l_y)
        angle_hu = np.arccos(cos_)
        return angle_hu

    def all_covered(self):
        flag = []
        for i in range(len(self.points)):
            flag.append(self.points[i].Ii)
        if 100 in flag:
            return False
        else:
            return True

    def inmap(self, point_around):
        i = 0
        while True:

            if point_around[i][0] < 0 or point_around[i][1] < 0 or point_around[i][0] >= self.gridMap.width or \
                    point_around[i][1] >= self.gridMap.length:
                point_around.pop(i)
            else:
                i = i + 1
            if i == len(point_around):
                break
        return point_around

    def find_around(self, index):
        point_around = []
        for i in range(len(self.direct)):
            point_around.append([self.points[index].x + self.direct[i][0], self.points[index].y + self.direct[i][1]])
        point_around = self.inmap(point_around)
        return point_around

    def F_fun(self, index):
        point_around = self.find_around(index)
        # print(point_around)
        temp_value = 0
        for i in range(len(point_around)):
            a_index = point_around[i][0] * 10 + point_around[i][1]
            temp_value = temp_value + self.W_fun(index, a_index) * self.points[a_index].f1
        self.points[index].oc = 1
        self.points[index].f1 = self.G_fun(temp_value + self.points[index].Ii)
        # print(self.points[index])

    def W_fun(self, index, a_index):
        de_x = self.points[index].x - self.points[a_index].x
        de_y = self.points[index].y - self.points[a_index].y
        distance = math.sqrt(de_x ** 2 + de_y ** 2)
        w_ij = math.exp(-2 * (distance ** 2))
        return w_ij

    def G_fun(self, value):
        if value < 0:
            return -1
        elif value >= 1:
            return 1
        else:
            return value * self.beta


if __name__ == '__main__':
    gridmap_1 = GirdMap.GridMap(10, 10)
    gbnn = GBNN(gridmap_1)
    path_aaa = gbnn.run(0)
    x_list = []
    y_list = []
    for i in range(len(path_aaa)):
        x_list.append(path_aaa[i][0])
        y_list.append(path_aaa[i][1])
    plt.plot(x_list, y_list)
    plt.show()
